#include "foc.h"
#include "arm_math_usr.h"
#include "stdio.h"
#include "tim.h"
#include "gpio.h"
#include "pid_config.h"

volatile uint32_t gFoc_TimeCNT;

TimEnc_Struct gSTTimerEncoder = {
    .Tim2Cnt = 0,
    .angle = 0,
};

void SetTIM1Channel1HighLeaveTime_us(float time);
void SetTIM1Channel2HighLeaveTime_us(float time);
void SetTIM1Channel3HighLeaveTime_us(float time);
void SetTIM1Channel4HighLeaveTime_us(float time);
static void SetMotorAndTimITEnable(uint8_t isEnable);
uint8_t GetMotorSVPWMSector(void);
float GetTimer2EncoderAngle(void);
static void GetMotorPreCurrent(float *ua, float *ub, float *uc);
void MotorSvpwmGenerate(float ualpha, float ubeta);
static void GetElectricalAngle(PFOC_Struct pFOC);



// 声明FOC对象
FOC_EXPORT(gMotorFOC, 4.0f, 3,
           SetMotorAndTimITEnable,
           GetTimer2EncoderAngle,
           GetMotorSVPWMSector,
           GetMotorPreCurrent,
           MotorSvpwmGenerate)

// 声明一个SVPWM对象
SVPWM_EXPORT(gMotor, 100.0f, 80.0f, 1.0f, 24.0f,
             SetTIM1Channel1HighLeaveTime_us,
             SetTIM1Channel2HighLeaveTime_us,
             SetTIM1Channel3HighLeaveTime_us,
             SetTIM1Channel4HighLeaveTime_us)

/*************************************************************
** Function name:       SetCurrentPIDParams
** Descriptions:        设置电流环参数
** Input parameters:    pFOC：FOC对象指针
**                      kp:比例
**                      ki:积分
**                      kd:微分
**                      outMax:输出限幅
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetCurrentPIDParams(PFOC_Struct pFOC, float kp, float ki, float kd, float outMax)
{
    pFOC->idPID.kp = kp;
    pFOC->idPID.ki = ki;
    pFOC->idPID.kd = kd;
    pFOC->idPID.outMax = outMax;

    pFOC->iqPID.kp = kp;
    pFOC->iqPID.ki = ki;
    pFOC->iqPID.kd = kd;
    pFOC->iqPID.outMax = outMax;
}

/*************************************************************
** Function name:       SetFocEnable
** Descriptions:        设置FOC使能
** Input parameters:    pFOC：FOC对象指针
**                      isEnable:是否使能
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetFocEnable(PFOC_Struct pFOC, uint8_t isEnable)
{
    pFOC->isEnable = isEnable;
    pFOC->SetEnable(pFOC->isEnable);
}

/*************************************************************
** Function name:       ClarkeTransform ,3 - 2 transform
** Descriptions:        Clarke正变换
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
** Ialpha = Ia, Ibeta = (2*Ib + Ia)/sqrt(3)
*************************************************************/
static void ClarkeTransform(PFOC_Struct pFOC)
{
    pFOC->ialpha = pFOC->ia;
    pFOC->ibeta = (pFOC->ia + 2.0f * pFOC->ib) / FOC_SQRT_3;
}

/*************************************************************
** Function name:       ParkTransform, 旋转至静止坐标变换，方便PID
** Descriptions:        Park正变换
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
** Id = Ialpha*cosθ - Ibeta*sinθ
** Iq = Ialpha*sinθ - Ibeta*cosθ
*************************************************************/
static void ParkTransform(PFOC_Struct pFOC)
{
    pFOC->id = pFOC->ialpha * arm_cos_f32(pFOC->radian) + pFOC->ibeta * arm_sin_f32(pFOC->radian);
    pFOC->iq = -pFOC->ialpha * arm_sin_f32(pFOC->radian) + pFOC->ibeta * arm_cos_f32(pFOC->radian);
}

/*************************************************************
** Function name:       ParkAntiTransform
** Descriptions:        Park反变换
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
** Ialpha = Id*cosθ - Iq*sinθ
** Ibeta  = Id*sinθ + Iq*cosθ
*************************************************************/
static void ParkAntiTransform(PFOC_Struct pFOC)
{
    pFOC->ialpha_SVPWM = pFOC->idPID.out * arm_cos_f32(pFOC->radian) - pFOC->iqPID.out * arm_sin_f32(pFOC->radian);
    pFOC->ibeta_SVPWM = pFOC->idPID.out * arm_sin_f32(pFOC->radian) + pFOC->iqPID.out * arm_cos_f32(pFOC->radian);
}

/*************************************************************
** Function name:       CurrentReconstruction
** Descriptions:        电流重构
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
static void CurrentReconstruction(PFOC_Struct pFOC)
{
    pFOC->GetPreCurrent(&pFOC->ia, &pFOC->ib, &pFOC->ic);
    if (pFOC->iNum < 3)
    {
        return;
    }
    switch (pFOC->GetSVPWMSector()) 
    {
        case 1:
            pFOC->ia =0.0f - pFOC->ib - pFOC->ic;
            break;
        case 2:
            pFOC->ib =0.0f - pFOC->ia - pFOC->ic;
            break;
        case 3:
            pFOC->ib =0.0f - pFOC->ia - pFOC->ic;
            break;
        case 4:
            pFOC->ic =0.0f - pFOC->ia - pFOC->ib;
            break;
        case 5:
            pFOC->ic =0.0f - pFOC->ia - pFOC->ib;
            break;
        case 6:
            pFOC->ia =0.0f - pFOC->ib - pFOC->ic;
            break;
        default:
            break;
    }
}


/*************************************************************
** Function name:       FocContorl
** Descriptions:        FOC控制流程
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void FocContorl(PFOC_Struct pFOC)
{
    // 0.获取电气角度
    GetElectricalAngle(pFOC);
    // 1计算实际电流值
    // 1.0电流重构
    CurrentReconstruction(pFOC);
    // 1.1Clarke变换
    ClarkeTransform(pFOC);
    // 1.2Park变换
    ParkTransform(pFOC);
    // 2.做PID闭环
    CurrentPIControlID(pFOC);
    CurrentPIControlIQ(pFOC);
    // pFOC->idPID.out = 0.0;
    // pFOC->iqPID.out = 2.0;
    // 3.计算输出值iα i贝塔
    ParkAntiTransform(pFOC);
    // 4.输出SVPWM
    pFOC->SvpwmGenerate(pFOC->ialpha_SVPWM, pFOC->ibeta_SVPWM);
    return;
}

SADC1_Struct gADC1 = {0};

/*************************************************************
** Function name:       GetADC1ChannelXValue
** Descriptions:        获取ADC通道 10 - 11的值
** Input parameters:    None
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
int32_t GetADC1ChannelXValue(uint8_t channel)
{
    int32_t value;
    value = (int32_t)(FILTER_KP * (float)gADC1.adValue[channel] + (1 - FILTER_KP) * (float)gADC1.lastAdValue[channel]);
    gADC1.lastAdValue[channel] = value;
    return value;
}

/*************************************************************
** Function name:       GetMotorPreCurrent
** Descriptions:        获取电机 3相实际电流
** Input parameters:    None
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
static void GetMotorPreCurrent(float *ua, float *ub, float *uc)
{
    *ua = GetADC1ChannelXValue(0) * AD_TO_CURRENT;
    *ub = GetADC1ChannelXValue(1) * AD_TO_CURRENT;
    *uc = GetADC1ChannelXValue(2) * AD_TO_CURRENT;

    //	*ua = GetADC1ChannelXValue(0);
    //	*ub = GetADC1ChannelXValue(1);
    //	*uc = GetADC1ChannelXValue(2);
}

/*************************************************************
** Function name:       SetMotorAndTimITEnable
** Descriptions:        设置电机和定时器使能
** Input parameters:    isEnable：1：使能 2：失能
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
static void SetMotorAndTimITEnable(uint8_t isEnable)
{
    // 设置驱动芯片使能
    //  SetMotorEnable(isEnable);
    // 设置定时器使能
    if (isEnable == 1)
    {
        HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4);
        HAL_TIM_Base_Start_IT(&htim1);
    }
    else
    {
        HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_4);
        HAL_TIM_Base_Stop_IT(&htim1);
    }
    //  Timer1Channel4ITEnable(isEnable);
}


/*************************************************************
** Function name:       GetTimer2EncoderCnt
** Descriptions:        获取定时器3编码器模式CNT的值
** Input parameters:    None
** Output parameters:   None
** Returned value:      Timer3->CNT的值
*************************************************************/
int32_t GetTimer2EncoderCnt(void)
{
    gSTTimerEncoder.Tim2Cnt += __HAL_TIM_GET_COUNTER(&htim2) - TIM2_CNT_CENT;
    __HAL_TIM_SET_COUNTER(&htim2, TIM2_CNT_CENT);
    if (gSTTimerEncoder.Tim2Cnt < 0)
    {
        gSTTimerEncoder.Tim2Cnt += TIM2_ENCODER_PLUSE_ONE_CYCLE;
    }
    gSTTimerEncoder.Tim2Cnt = gSTTimerEncoder.Tim2Cnt % (TIM2_ENCODER_PLUSE_ONE_CYCLE/4);
    return gSTTimerEncoder.Tim2Cnt;
}

/*************************************************************
** Function name:       GetTimerEncoderAngle
** Descriptions:        获取编码器角度
** Input parameters:    None
** Output parameters:   None
** Returned value:      编码器角度
*************************************************************/
float GetTimer2EncoderAngle(void)
{
    // int32_t tim2_count_temp = GetTimer2EncoderCnt()/TIM2_ENCODER_PLUSE_ONE_CYCLE;
    gSTTimerEncoder.angle = (float)GetTimer2EncoderCnt() / (float)TIM2_ENCODER_PLUSE_ONE_CYCLE * 360.0f; //
    if (gSTTimerEncoder.angle < 0)
    {
        gSTTimerEncoder.angle = gSTTimerEncoder.angle + 360.0f;
    }
    return gSTTimerEncoder.angle;
}


/*************************************************************
** Function name:       GetElectricalAngle
** Descriptions:        获取电气角度
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
static void GetElectricalAngle(PFOC_Struct pFOC)
{
    pFOC->angle = pFOC->GetEncoderAngle() * pFOC->polePairs;
    // pFOC->angle = pFOC->GetEncoderAngle();
    pFOC->radian_temp = pFOC->angle * FOC_ANGLE_TO_RADIN ;
}


/*************************************************************
** Function name:       SectorJudgment
** Descriptions:        扇区判断,利用uα和uβ来判断扇区
** Input parameters:    pSvpwm：结构体名字
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
#define SVPWM_SQRT3_2 0.866f
#define SVPWM_SQRT3 1.732f

void SvpwmSectorJudgment(PSvpwm_Struct pSvpwm)
{
    uint8_t a;
    uint8_t b;
    uint8_t c;
    uint8_t sector;

    pSvpwm->u1 = pSvpwm->ubeta;
    // pSvpwm->u2 = pSvpwm->uα * sqrt(3) / 2 - pSvpwm->uβ / 2;
    // pSvpwm->u3 = -pSvpwm->uα * sqrt(3) / 2 - pSvpwm->uβ / 2;
    pSvpwm->u2 = pSvpwm->ualpha * SVPWM_SQRT3_2 - pSvpwm->ubeta / 2;
    pSvpwm->u3 = -pSvpwm->ualpha * SVPWM_SQRT3_2 - pSvpwm->ubeta / 2;

    if (pSvpwm->u1 > 0)
    {
        a = 1;
    }
    else
    {
        a = 0;
    }
    if (pSvpwm->u2 > 0)
    {
        b = 1;
    }
    else
    {
        b = 0;
    }
    if (pSvpwm->u3 > 0)
    {
        c = 1;
    }
    else
    {
        c = 0;
    }

    sector = 4 * c + 2 * b + a;
    switch (sector)
    {
    case 3:
        pSvpwm->sector = 1;
        break;
    case 1:
        pSvpwm->sector = 2;
        break;
    case 5:
        pSvpwm->sector = 3;
        break;
    case 4:
        pSvpwm->sector = 4;
        break;
    case 6:
        pSvpwm->sector = 5;
        break;
    case 2:
        pSvpwm->sector = 6;
        break;
    }
}

/*************************************************************
** Function name:       GetVectorDuration
** Descriptions:        获取矢量作用时长
** Input parameters:    pSvpwm:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void GetVectorDuration(PSvpwm_Struct pSvpwm)
{
    switch (pSvpwm->sector)
    {
    case 1:
        pSvpwm->t4 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
        pSvpwm->t6 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
        pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t4 - pSvpwm->t6) / 2;
        break;
    case 2:
        pSvpwm->t2 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
        pSvpwm->t6 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
        pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t2 - pSvpwm->t6) / 2;
        break;
    case 3:
        pSvpwm->t2 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
        pSvpwm->t3 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
        pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t2 - pSvpwm->t3) / 2;
        break;
    case 4:
        pSvpwm->t1 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
        pSvpwm->t3 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
        pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t1 - pSvpwm->t3) / 2;
        break;
    case 5:
        pSvpwm->t1 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
        pSvpwm->t5 = SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u2;
        pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t1 - pSvpwm->t5) / 2;
        break;
    case 6:
        pSvpwm->t4 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u3;
        pSvpwm->t5 = -SVPWM_SQRT3 * pSvpwm->ts / pSvpwm->udc * pSvpwm->u1;
        pSvpwm->t0 = pSvpwm->t7 = (pSvpwm->ts - pSvpwm->t4 - pSvpwm->t5) / 2;
        break;
    default:
        break;
    }
}

/*************************************************************
** Function name:       SvpwmGenerate
** Descriptions:        SVPWM生成周期函数
** Input parameters:    pSvpwm:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SvpwmGenerate(PSvpwm_Struct pSvpwm)
{
    float ta = 0.0f;
    float tb = 0.0f;
    float tc = 0.0f;
    float td = 0.0f;
    switch (pSvpwm->sector)
    {
    case 1:
        ta = pSvpwm->t4 + pSvpwm->t6 + pSvpwm->t7;
        tb = pSvpwm->t6 + pSvpwm->t7;
        tc = pSvpwm->t7;
        break;
    case 2:
        ta = pSvpwm->t6 + pSvpwm->t7;
        tb = pSvpwm->t2 + pSvpwm->t6 + pSvpwm->t7;
        tc = pSvpwm->t7;
        break;
    case 3:
        ta = pSvpwm->t7;
        tb = pSvpwm->t2 + pSvpwm->t3 + pSvpwm->t7;
        tc = pSvpwm->t3 + pSvpwm->t7;
        break;
    case 4:
        ta = pSvpwm->t7;
        tb = pSvpwm->t3 + pSvpwm->t7;
        tc = pSvpwm->t1 + pSvpwm->t3 + pSvpwm->t7;
        break;
    case 5:
        ta = pSvpwm->t5 + pSvpwm->t7;
        tb = pSvpwm->t7;
        tc = pSvpwm->t1 + pSvpwm->t5 + pSvpwm->t7;
        break;
    case 6:
        ta = pSvpwm->t4 + pSvpwm->t5 + pSvpwm->t7;
        tb = pSvpwm->t7;
        tc = pSvpwm->t5 + pSvpwm->t7;
        break;
    }
    td = ta;
    if (td < tb)
    {
        td = tb;
    }
    if (td < tc)
    {
        td = tc;
    }

    if (td > pSvpwm->maxTs)
    {
        ta = ta * td / pSvpwm->maxTs;
        tb = tb * td / pSvpwm->maxTs;
        tc = tc * td / pSvpwm->maxTs;
        td = pSvpwm->maxTs + pSvpwm->adcTs;
    }
    //	ta = tb = tc = 0;
    //	td = 50;
    pSvpwm->SetChannelAHighLeaveTime_us(ta);
    pSvpwm->SetChannelBHighLeaveTime_us(tb);
    pSvpwm->SetChannelCHighLeaveTime_us(tc);
    pSvpwm->SetChannelDHighLeaveTime_us(td);
}

/*************************************************************
** Function name:       MotorSvpwmGenerate
** Descriptions:        电机SVPWM生成
** Input parameters:    None
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void MotorSvpwmGenerate(float ualpha, float ubeta)
{
    SvpwmContorol(&gMotor, ualpha, ubeta);
}

/*************************************************************
** Function name:       SvpwmContorol
** Descriptions:        Svpwm控制
** Input parameters:    pSvpwm:结构体指针
**                      uα：阿尔法轴目标电压
**                      uβ：贝塔轴目标电压
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SvpwmContorol(PSvpwm_Struct pSvpwm, float ualpha, float ubeta)
{
    pSvpwm->ualpha = ualpha;
    pSvpwm->ubeta = ubeta;
    // 1.扇区判断
    SvpwmSectorJudgment(pSvpwm);
    // 1.计算矢量作用时长
    GetVectorDuration(pSvpwm);
    // 1.SVPWM生成
    SvpwmGenerate(pSvpwm);
}

/*************************************************************
** Function name:       GetSVPWMSector
** Descriptions:        获取SVPWM扇区
** Input parameters:    None
** Output parameters:   None
** Returned value:      SVPWM扇区(1-6)
** Remarks:             None
*************************************************************/
uint8_t GetSVPWMSector(PSvpwm_Struct pSvpwm)
{
    return pSvpwm->sector;
}

/*************************************************************
** Function name:       GetMotorSVPWMSector
** Descriptions:        获取电机的扇区
** Input parameters:    None
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
uint8_t GetMotorSVPWMSector(void)
{
    return GetSVPWMSector(&gMotor);
}

/*************************************************************
** Function name:       SetTIM1Channel1HighLeaveTime_us
** Descriptions:        设置定时器1通道1高电平时间（中央对齐模式二）
** Input parameters:    time：高电平时间 单位us 范围 0-100
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetTIM1Channel1HighLeaveTime_us(float time)
{
    uint32_t ccr;
    if (time < 0)
    {
        time = 0;
    }
    if (time > 100)
    {
        time = 100;
    }
    ccr = 42 * time;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, ccr);
}

/*************************************************************
** Function name:       SetTIM1Channel2HighLeaveTime_us
** Descriptions:        设置定时器1通道2高电平时间（中央对齐模式二）
** Input parameters:    time：高电平时间 单位us 范围 0-100
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetTIM1Channel2HighLeaveTime_us(float time)
{
    uint32_t ccr;
    if (time < 0)
    {
        time = 0;
    }
    if (time > 100)
    {
        time = 100;
    }
    ccr = 42 * time;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, ccr);
}

/*************************************************************
** Function name:       SetTIM1Channel3HighLeaveTime_us
** Descriptions:        设置定时器1通道3高电平时间（中央对齐模式二）
** Input parameters:    time：高电平时间 单位us 范围 0-100
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetTIM1Channel3HighLeaveTime_us(float time)
{
    uint32_t ccr;
    if (time < 0)
    {
        time = 0;
    }
    if (time > 100)
    {
        time = 100;
    }
    ccr = 42 * time;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, ccr);
}

/*************************************************************
** Function name:       SetTIM1Channel4HighLeaveTime_us
** Descriptions:        设置定时器1通道4高电平时间（中央对齐模式二）
** Input parameters:    time：高电平时间 单位us 范围 0-100
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetTIM1Channel4HighLeaveTime_us(float time)
{
    uint32_t ccr;
    if (time < 0)
    {
        time = 0;
    }
    if (time > 100)
    {
        time = 100;
    }
    ccr = 42 * time;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, ccr);
}



void foc_test_init(void)
{
    gMotorFOC.tariq = 0.5f;
    gMotorFOC.tarid = 0.0f;
    SetCurrentPIDParams(&gMotorFOC,M_KP,M_KI,M_KD,M_OUTMAX);

    SetTIM1Channel1HighLeaveTime_us(55);
	SetTIM1Channel2HighLeaveTime_us(45);
	SetTIM1Channel3HighLeaveTime_us(35);
    gMotor.sector = 1;
	HAL_Delay(500);
    for (uint8_t i = 0; i < 10; i++) {
		__HAL_TIM_SET_COUNTER(&htim2,TIM2_CNT_CENT);
        gSTTimerEncoder.angle = PI_ / 3.0;
        gSTTimerEncoder.Tim2Cnt = 0;
		HAL_Delay(50);
	}
    SetTIM1Channel1HighLeaveTime_us(0);
	SetTIM1Channel2HighLeaveTime_us(0);
	SetTIM1Channel3HighLeaveTime_us(0);

    gMotorFOC.isEnable = ENABLE;
    gMotorFOC.SetEnable(gMotorFOC.isEnable);

}

// float motor_angle_w = 0.0f;

void foc_test(void)
{
    static float motor_w = 0.0;
    if (motor_w > (2 * PI_))
    {
        motor_w = 0.0;
    }
    motor_w += 0.0005;
    gMotorFOC.radian = motor_w;


    // motor_angle_w = GetTimer2EncoderAngle() * FOC_ANGLE_TO_RADIN * 8;
    GetElectricalAngle(&gMotorFOC);
    CurrentReconstruction(&gMotorFOC);
    ClarkeTransform(&gMotorFOC);
    ParkTransform(&gMotorFOC);
    // CurrentPIControlID(&gMotorFOC);
    // CurrentPIControlIQ(&gMotorFOC);
    gMotorFOC.idPID.out = 0.0f;
    gMotorFOC.iqPID.out = 2.0f;
    ParkAntiTransform(&gMotorFOC);

    gMotorFOC.SvpwmGenerate(gMotorFOC.ialpha_SVPWM, gMotorFOC.ibeta_SVPWM);
    // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 500);
    // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 800);
    // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 1200);
    // HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_8);
    // __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 1500);

    // GetTimer2EncoderCnt();
    
}

void foc_test_printf(void)
{
    printf("%0.2f %4d, \
            %0.2f %0.2f %0.2f, \
            %0.2f %0.2f,\
            %0.2f %0.2f,\
            %4d %4d %4d\r\n", \
            (float)gMotorFOC.radian_temp, (int)gMotor.sector,\
            (float)gMotorFOC.ia, (float)gMotorFOC.ib, (float)gMotorFOC.ic,\
            (float)gMotorFOC.id, (float)gMotorFOC.iq,\
            (float)gMotorFOC.idPID.out, (float)gMotorFOC.iqPID.out,\
            (int)TIM1->CCR1, (int)TIM1->CCR2, (int)TIM1->CCR3);
}